#include "Octree.h"

/**************************************************************************************************
* Constructor
**************************************************************************************************/
Octree::Octree(void) {
	// Initialize the root node
	root = new OctreeNode();

	depth = 0;
}

/**************************************************************************************************
* Destructor
**************************************************************************************************/
Octree::~Octree(void) {
//	delete root;
}

/**************************************************************************************************
* Build the basis for a tree
**************************************************************************************************/
OctreeNode* Octree::buildTree(Vector3 center, Vector3 halfWidth, int stopDepth) {
	// If there are no elements, return
	if(stopDepth <= 0)
		return 0;

	// Update total depth of tree
	if(stopDepth > depth)
		depth = stopDepth;

	// Construct and fill in 'root' the of this subtree
	OctreeNode* node = new OctreeNode();
	node->center.x = center.x;
	node->center.y = center.y;
	node->center.z = center.z;
	node->halfWidth = halfWidth;

	// Recursively construct the eight children of the subtree
	Vector3 offset;
	Vector3 step = halfWidth * 0.5f;
	for(int i = 0; i < 8; i++) {
		offset.x = ((i & 1) ? step.x : -step.x);
		offset.y = ((i & 2) ? step.y : -step.y);
		offset.z = ((i & 4) ? step.z : -step.z);
		for(int j = 0; j < 3; j++)
			offset[j] = offset[j] + center[j];
		node->child[i] = buildTree(offset, step, stopDepth - 1);
	}

	return node;
}

/**************************************************************************************************
* Insert an object into the tree
**************************************************************************************************/
void Octree::insertObject(OctreeNode* tree, SmurfsPolygon* obj, int currDepth) {
	int straddle = 0;	// If the object is straddling two or more child nodes
	int index = 0;		// Index of child

	// calculate radius of polygon
	float radius;

	float c0 = (obj->getPoints()[0] - obj->center).magnitude();
	float c1 = (obj->getPoints()[1] - obj->center).magnitude();
	float c2 = (obj->getPoints()[2] - obj->center).magnitude();

	if(c0 > c1 && c0 > c2)
		radius = c0;
	else if(c1 > c0 && c1 > c2)
		radius = c1;
	else
		radius = c2;

	// Compute the octant number [0..7] the object sphere center is in. If straddling any of the 
	// dividing planes, exit directly.
	for(int i = 0; i < 3; i++) {
		float delta = obj->center[i] - tree->center[i];
		if(std::abs(delta) <= radius) {
			straddle = 1;
			break;
		}
		
		if(delta > 0.0f)
			index |= (1 << i);	// ZYX
	}

	if(!straddle) {
		if(tree->child[index] == 0) {
			// Initialize new node
			tree->polygons.push_back(obj);
			obj->setRadius(radius);
		} else {
			// Fully contained in existing child node; insert in that subtree
			insertObject(tree->child[index], obj, currDepth + 1);
		}
	} else {
		// Straddling, or no child node to descend into so link object into linked list at this node
		tree->polygons.push_back(obj);
		obj->setRadius(radius);
	}
}

/**************************************************************************************************
* Determines whether the node can still contain the object in question or whether it lies outside.
**************************************************************************************************/
bool Octree::nodeContainable(OctreeNode* tree, SmurfsPolygon* obj) {
	// Find the distance from the center of the node
	float dist;
	for(int i = 0; i < 3; i++) {
		dist = obj->center[i] - tree->center[i];
		
		// if at any point the distance is further than the reach of the child, it is no longer
		// containable
//		if(abs(dist) > tree->halfWidth)
//			return false;
	}

	// All checks passed, it is contained
	return true;
}	

/**************************************************************************************************
* Removes the object from a node by altering its linked list.
**************************************************************************************************/
void Octree::removeObject(SmurfsPolygon* obj) { }

/**************************************************************************************************
* Queries the tree for any objects that are from any nodes that encompass a particular point
**************************************************************************************************/
void Octree::query(OctreeNode* tree, SmurfsPolygon* obj, Vector3 position) {
	// Iterate through any items in the node's object list
	/*OctreeObject<Type>* tObj = tree->objList;

	while(tObj != 0) {
		// Set the data and add it to the linked list
		obj->data = tObj->data;
		ARGolf::MemoryManager::getSingletonPtr()->allocate(sizeof(OctreeObject<Type>), 0);
		obj->nextObject = new OctreeObject<Type>();
		obj->nextObject->previousObject = obj;
		obj = obj->nextObject;
		tObj = tObj->nextObject;
	}

	// Determine which child index to descend into
	int index = 0;		// Index of child
	int straddle = 0;	// If the object is straddling two or more child nodes

	// Compute the octant number [0..7] the object sphere center is in. If straddling any of the 
	// dividing planes, exit directly.
	for(int i = 0; i < 3; i++) {
		float delta = obj->center[i] - tree->center[i];
		if(std::abs(delta) <= obj->radius) {
			straddle = 1;
			break;
		}
		
		if(delta > 0.0f)
			index |= (1 << i);	// ZYX
	}

	// Query the child assuming it exists
	if(tree->child[index] != 0) {
		query(tree->child[index], obj, position);
	}*/
}